/*
   Copywrite 2013. All rights reserved.
   CyPhy Lab, https://wiki.qut.edu.au/display/cyphy
   Queensland University of Technology
   Brisbane, Australia

Author: Feras Dayoub

Redistribution and use in source and binary forms, with or 
without modification, are permitted provided that the following 
conditions are met:

Redistributions of source code must retain the above copyright notice, 
this list of conditions and the following disclaimer. 

Redistributions in binary form must reproduce the above copyright notice, 
this list of conditions and the following disclaimer in the documentation 
and/or other materials provided with the distribution. 

Neither the name of the Queensland University of Technology, CyPhy Lab nor the names of 
the contributors may be used to endorse or promote products derived 
from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 
THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
OF SUCH DAMAGE.
 */

#include "ros/ros.h"
#include <ros/console.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/TwistStamped.h"
#include <nav_msgs/Odometry.h>
#include "tf/transform_listener.h"
#include "tf/tf.h"
#include "local_mapper/GetLocalMap.h"
#include "localizer/Switched.h"
#include "localizer/SwitchPending.h"
#include "topometric_planner/GetLocalGoal.h"
#include "localizer/TestSwitch.h"
#include "std_msgs/Bool.h"
#include <iostream>
#include <boost/thread/mutex.hpp>
#include <cyphy_vslam_msgs/GlobalLocalisation.h>
#include <slam_backend/GetNodePose.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>


using namespace std;
const double PI = std::atan(1.0) * 4;

class Localizer {
private:
	ros::NodeHandle nh_;
	std::vector<geometry_msgs::Pose2D> inner_nodes_poses;
	std::vector<int> inner_nodes_ids;
	int gateway_idx;
	int init_node_id_;
	//MSGS
	geometry_msgs::PoseWithCovarianceStamped init_pose;
	geometry_msgs::PoseWithCovarianceStamped local_pose;
	geometry_msgs::PoseWithCovarianceStamped global_pose;
	geometry_msgs::Pose2D local_goal_pose_;
	//transformers
	tf::TransformListener *tf_listener;
	tf::StampedTransform current_world_to_map_transform;
	tf::StampedTransform previous_world_to_map_transform;
	//publishers
	ros::Publisher initpose_pub;
	ros::Publisher local_pose_pub;
	ros::Publisher global_pose_pub;
	ros::Publisher switch_pending_pub;
	ros::Publisher switched_pub;
	//service server
	ros::ServiceServer switch_test;
	//SERVICE CLIENTS
	ros::ServiceClient local_mapper_client;
	ros::ServiceClient local_goal_client;
	ros::ServiceClient slamBackend_client;
	//SERVICES
	local_mapper::GetLocalMap local_map_srv;
	topometric_planner::GetLocalGoal local_goal_srv;
	slam_backend::GetNodePose get_node_pose_srv;


	std::string local_goal_srv_name_;
	std::string switch_map_srv_name_;
	//subscribers
	ros::Subscriber local_pose_sub;
	ros::Subscriber apperance_localization_sub;
	ros::Subscriber odomSub_;
	//ros::Subscriber local_goal;

	//topics
	std::string amcl_pose_;
	std::string local_goal_;
	//frames
	std::string local_frame_;
	std::string global_frame_;

	//param
	double dist_to_switch_;
	//boolean switch
	bool have_goal_;
	bool offline_mode_;
	boost::mutex m_lock_;
	int current_goal_id_;
	bool inialized;


public:

	Localizer(ros::NodeHandle n) :
			nh_(n) {

		amcl_pose_ = nh_.resolveName("amcl_pose");
		switch_map_srv_name_ = nh_.resolveName("switch_map");
		//local_goal_ = nh_.resolveName("local_goal");
		local_goal_srv_name_ = nh_.resolveName("get_local_goal");

		ros::NodeHandle n_local_("~");
		n_local_.param<double>("dist_to_switch", dist_to_switch_, 0.5);
		n_local_.param<int>("init_node_id", init_node_id_, 0);
		n_local_.param<std::string>("local_frame", local_frame_, "local_map");
		n_local_.param<bool>("offline_mode", offline_mode_, false);
		n_local_.param<std::string>("global_frame", global_frame_,
				"global_map");
		//offline_mode_ = true;
		ROS_INFO("offline_mode is %s", (offline_mode_)?"true":"false");

		//cout << dist_to_switch_ << endl;
		set_init_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
		local_pose = init_pose;
		global_pose.header.frame_id = local_frame_;
		global_pose = init_pose;
		global_pose.header.frame_id = global_frame_;

		local_pose_sub = nh_.subscribe(amcl_pose_, 1, &Localizer::amclCB, this);

		std::string apper_localization = nh_.resolveName("appearance_based_localization");
		apperance_localization_sub = nh_.subscribe(apper_localization, 1, &Localizer::appLocCB, this);


		//local_goal = nh_.subscribe(local_goal_,1,&Localizer::set_local_goal,this);

		local_mapper_client = nh_.serviceClient<local_mapper::GetLocalMap>(
				switch_map_srv_name_);

		local_goal_client = nh_.serviceClient<topometric_planner::GetLocalGoal>(
				local_goal_srv_name_);

		std::string get_node_pose_srv_name = nh_.resolveName("get_node_pose");
		slamBackend_client = nh_.serviceClient<slam_backend::GetNodePose>(
				get_node_pose_srv_name);

		initpose_pub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
				"initialpose", 1);
		local_pose_pub =
				nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
						"local_pose", 1);
		global_pose_pub =
				nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
						"global_pose", 1);
		switch_pending_pub = nh_.advertise<localizer::SwitchPending>(
				"switch_pending", 1);
		switched_pub = nh_.advertise<std_msgs::Bool>("switched", 1);

		switch_test = nh_.advertiseService("switch_test",
				&Localizer::switch_test_CB_, this);

		tf_listener = new tf::TransformListener();


	}


	void appLocCB(const cyphy_vslam_msgs::GlobalLocalisation msg){

		std::cout << "\n * \n * Received Global localization message says I am near node: " << msg.node_id << " \n * \n *" ;

		get_node_pose_srv.request.node_id = msg.node_id;
		if (slamBackend_client.call(get_node_pose_srv)) {
			geometry_msgs::Pose2D app_based_pose = get_node_pose_srv.response.global_pose;

			geometry_msgs::PoseStamped pLocal, pGlobal;
			pGlobal.pose.position.x = app_based_pose.x;
			pGlobal.pose.position.y = app_based_pose.y;
			pGlobal.pose.position.z = 0;
			tf::Quaternion q;
			q.setRotation(tf::Vector3(0, 0, 1),app_based_pose.theta);
			pGlobal.pose.orientation.z = q[2];
			pGlobal.pose.orientation.w = q[3];
			pGlobal.pose.orientation.x = 0;
			pGlobal.pose.orientation.y = 0;

			pGlobal.header.frame_id = global_pose.header.frame_id;
			pGlobal.header.stamp = msg.header.stamp;

					try {
						tf_listener->waitForTransform(local_frame_, global_frame_,
								ros::Time::now(), ros::Duration(3.0));
						tf_listener->transformPose(local_frame_, pGlobal, pLocal);
						local_pose.pose.pose = pLocal.pose;
						local_pose.header.stamp = global_pose.header.stamp;
						local_pose.header.frame_id = local_frame_;
					} catch (tf::TransformException &ex) {
						ROS_ERROR("%s", ex.what());
					}
					float dx = this->local_pose.pose.pose.position.x - pLocal.pose.position.x;
					float dy = this->local_pose.pose.pose.position.y - pLocal.pose.position.y;
					float dd = std::sqrt(std::pow(dx,2) + std::pow(dy,2));

					if (dd > 1.0){
					ROS_INFO("resetting pose based on appearance.");

											set_init_pose(pLocal.pose.position.x,
											                pLocal.pose.position.y,
													pLocal.pose.position.z,
													pLocal.pose.orientation.w,
													pLocal.pose.orientation.x,
													pLocal.pose.orientation.y,
													pLocal.pose.orientation.z);
											init_pose.header.stamp = pLocal.header.stamp;

											initpose_pub.publish(init_pose);

											inialized = true;
					}

		}

	}

	bool switch_test_CB_(localizer::TestSwitch::Request &req,
			localizer::TestSwitch::Response &res) {
                have_goal_ = false;
		// I am switching maps
		local_map_srv.request.node_id = req.node_id;
		// let the planner know
		std_msgs::Bool swp_msg;
		swp_msg.data = true;
		switch_pending_pub.publish(swp_msg);
		if (local_mapper_client.call(local_map_srv)) {
			transform_globalpose_to_localpose();
			ROS_INFO("resetting the initial pose.");
            set_init_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
			initpose_pub.publish(init_pose);
			// tell the planner
			std_msgs::Bool sw_msg;
			sw_msg.data = true;
			switched_pub.publish(sw_msg);
			res.a = true;

		} else {
			ROS_ERROR("Failed to call service get_local_map");
		}
		return true;
	}

	void set_local_goal(const geometry_msgs::Pose2D msg) {
		local_goal_pose_ = msg;
		ROS_INFO(
				"Current Goal: x=%f, y=%f", local_goal_pose_.x, local_goal_pose_.y);
	}

	void set_init_pose(float x, float y, float z, float ori_w, float ori_x,
			float ori_y, float ori_z) {
		init_pose.pose.pose.position.x = x;
		init_pose.pose.pose.position.y = y;
		init_pose.pose.pose.position.z = z;
		init_pose.pose.pose.orientation.w = ori_w;
		init_pose.pose.pose.orientation.x = ori_x;
		init_pose.pose.pose.orientation.y = ori_y;
		init_pose.pose.pose.orientation.z = ori_z;
		init_pose.pose.covariance = local_pose.pose.covariance;
		init_pose.header.stamp = ros::Time::now();
		init_pose.header.frame_id = local_frame_;
	}

	void amclCB(const geometry_msgs::PoseWithCovarianceStamped msg) {

		if (!inialized) {
			local_pose = msg;
			local_pose.header.frame_id = local_frame_;
			local_pose.header.stamp = msg.header.stamp;
			transform_localpose_to_globalpose();
			global_pose_pub.publish(global_pose);
			local_pose_pub.publish(local_pose);
		} else {
			inialized = false;
		}
	}

	void print_global_pose() {
		ROS_INFO(
				"Global pose: x=%f, y=%f ,theta=%f", global_pose.pose.pose.position.x, global_pose.pose.pose.position.y, 2 * std::asin(global_pose.pose.pose.orientation.z) * 180 / PI);

	}

	void print_local_pose() {
		ROS_INFO(
				"Local pose: x=%f, y=%f ,theta=%f", local_pose.pose.pose.position.x, local_pose.pose.pose.position.y, 2 * std::asin(local_pose.pose.pose.orientation.z) * 180 / PI);

	}

	void transform_localpose_to_globalpose() {
		geometry_msgs::PoseStamped pLocal, pGlobal;
		pLocal.pose = local_pose.pose.pose;
		pLocal.header.frame_id = local_pose.header.frame_id;

		try {
			tf_listener->waitForTransform(global_frame_, local_frame_,
					ros::Time::now(), ros::Duration(5.0));
			tf_listener->transformPose(global_frame_, pLocal, pGlobal);
			global_pose.pose.pose = pGlobal.pose;
			global_pose.header.stamp = local_pose.header.stamp;

		} catch (tf::TransformException &ex) {

			ROS_ERROR("%s", ex.what());
		}


	}

	geometry_msgs::PoseStamped transform_globalpose_to_localpose() {
		geometry_msgs::PoseStamped pLocal, pGlobal;
		pGlobal.pose = global_pose.pose.pose;
		pGlobal.header.frame_id = global_pose.header.frame_id;
		try {
			tf_listener->waitForTransform(local_frame_, global_frame_,
					ros::Time::now(), ros::Duration(5.0));
			tf_listener->transformPose(local_frame_, pGlobal, pLocal);
			boost::mutex::scoped_lock l(m_lock_);
			local_pose.pose.pose = pLocal.pose;
			local_pose.header.stamp = global_pose.header.stamp;
			local_pose.header.frame_id = local_frame_;
			previous_world_to_map_transform = current_world_to_map_transform;
		} catch (tf::TransformException &ex) {
			ROS_ERROR("%s", ex.what());
		}
          return pLocal;
	}

	void transform_goal_to_local_frame() {
		ROS_INFO(
				"Current Goal in global frame: x=%f, y=%f", local_goal_pose_.x, local_goal_pose_.y);
		geometry_msgs::PoseStamped pLocal, pGlobal;
		pGlobal.pose.position.x = local_goal_pose_.x;
		pGlobal.pose.position.y = local_goal_pose_.y;
		pGlobal.pose.position.z = 0; // goal in 2d.
		pGlobal.pose.orientation.z = 1;
		pGlobal.header.frame_id = global_frame_;

		try {

			tf_listener->waitForTransform(local_frame_, global_frame_,
					ros::Time::now(), ros::Duration(5.0));
			tf_listener->transformPose(local_frame_, pGlobal, pLocal);
			boost::mutex::scoped_lock l(m_lock_);
			local_goal_pose_.x = pLocal.pose.position.x;
			local_goal_pose_.y = pLocal.pose.position.y;

		} catch (tf::TransformException &ex) {
			ROS_ERROR("%s", ex.what());
		}
		ROS_INFO(
				"Current Goal in local frame: x=%f, y=%f", local_goal_pose_.x, local_goal_pose_.y);
		//print_local_pose();
	}

	double distance_to_goal() {

		double rx = local_pose.pose.pose.position.x;
		double ry = local_pose.pose.pose.position.y;
		double dx = std::pow(rx - local_goal_pose_.x, 2);
		double dy = std::pow(ry - local_goal_pose_.y, 2);
		return std::sqrt(dx + dy);
	}

	bool init() {
		have_goal_ = false;

		// call local_mapper service
		local_map_srv.request.node_id = init_node_id_;

		ROS_INFO(
				"Asking for the first local map centered on node:%d", init_node_id_);

		// let the planner know
		std_msgs::Bool swp_msg;
		swp_msg.data = true;
		switch_pending_pub.publish(swp_msg);

		if (local_mapper_client.call(local_map_srv)) {
			ROS_INFO("New map received");
			// tell the planner
			std_msgs::Bool sw_msg;
			sw_msg.data = true;
			switched_pub.publish(sw_msg);

			if (local_goal_client.call(local_goal_srv)) {
				if (local_goal_srv.response.goal_id != -1) {
					local_goal_pose_ = local_goal_srv.response.local_goal;
					gateway_idx = local_goal_srv.response.map_centre;
					transform_goal_to_local_frame();
					current_goal_id_ = local_goal_srv.response.goal_id;
					ROS_WARN("Goal obtained on node %d", current_goal_id_);
					have_goal_ = true;
				} else {
					ROS_WARN("Failed to obtain a goal.");
					have_goal_ = false;
				}
			} else {
				ROS_WARN("Failed to contact the planer.");
				have_goal_ = false;
			}

			// resetting the initial pose.
			ROS_INFO("resetting the initial pose.");

			set_init_pose(local_pose.pose.pose.position.x,
					local_pose.pose.pose.position.y,
					local_pose.pose.pose.position.z,
					local_pose.pose.pose.orientation.w,
					local_pose.pose.pose.orientation.x,
					local_pose.pose.pose.orientation.y,
					local_pose.pose.pose.orientation.z);

			initpose_pub.publish(init_pose);
			inialized = true;
			print_local_pose();

		} else {
			return false;
		}
		return true;
	}

	void run() {
		ros::Rate loop_rate(5);
		while (ros::ok()) {
			local_pose_pub.publish(local_pose);
			if (!have_goal_) {
				if (local_goal_client.call(local_goal_srv)) {
					if (local_goal_srv.response.goal_id != -1) {
						local_goal_pose_ = local_goal_srv.response.local_goal;
						gateway_idx = local_goal_srv.response.map_centre;
						transform_goal_to_local_frame();
						current_goal_id_ = local_goal_srv.response.goal_id;
						ROS_INFO("Goal obtained on node %d", current_goal_id_);
						have_goal_ = true;
					} else {
						ROS_WARN("Failed to obtain a goal.");
						have_goal_ = false;
					}
				} else {
					ROS_WARN("Failed to contact the planner.");
					have_goal_ = false;
				}
			} else {
				double d = distance_to_goal();
				ROS_INFO(
						"distance to goal is %f at node %d", d, current_goal_id_);
				if (gateway_idx ==-1){
					have_goal_ = false;
                    ros::Duration(30).sleep();
				}else if (d < dist_to_switch_) {
					// I am switching maps
					local_map_srv.request.node_id = gateway_idx;
					ROS_INFO(
							"Asking for a new local map centered on node:%d", gateway_idx);
					// let the planner know
					std_msgs::Bool swp_msg;
					swp_msg.data = true;
					switch_pending_pub.publish(swp_msg);
					if (local_mapper_client.call(local_map_srv)) {
						ROS_INFO("New map received");
						ROS_INFO("Transfereing global pose to local pose.");
					    geometry_msgs::PoseStamped pLocal = transform_globalpose_to_localpose();
						// resetting the initial pose.
						set_init_pose(pLocal.pose.position.x,
						                pLocal.pose.position.y,
								pLocal.pose.position.z,
								pLocal.pose.orientation.w,
								pLocal.pose.orientation.x,
								pLocal.pose.orientation.y,
								pLocal.pose.orientation.z);
						init_pose.header.stamp = pLocal.header.stamp;
						initpose_pub.publish(init_pose);
						inialized = true;
						// tell the planner
						std_msgs::Bool sw_msg;
						sw_msg.data = true;
						switched_pub.publish(sw_msg);
						have_goal_ = false;

					} else {
						ROS_ERROR("Failed to call service get_local_map");

					}

				}
			}
			ros::spinOnce();
			loop_rate.sleep();
		}
	}

};

int main(int argc, char** argv) {
	ros::init(argc, argv, "localizer");
	ros::NodeHandle n;
    ros::service::waitForService("switch_map");
	Localizer *localizer = new Localizer(n);

	while (!localizer->init()) {
		ROS_WARN("Not able to contact local_mapper.");
		sleep(1);
	}
	localizer->run();
	delete localizer;
	return 0;
}

